#include <Init_dev.hpp>

SetPWM LMotorPWM(PWM2, 100000, 0, "inversed"); // gpio66
SetPWM RMotorPWM(PWM1, 100000, 0, "normal");   // gpio65
// Servo
GtimPwm servo(88, 2, LS_GTIM_INVERSED, 2000000 - 1, 157500);
// 负压
GtimPwm NP(89, 3, LS_GTIM_INVERSED, 2000000 - 1, 155000);
// Encoder
LS_PwmEncoder EncodersL(0, 73), EncodersR(3, 72);
// GPIO(按键)
HWGpio Gpio1(44, GPIO_Mode_In);
HWGpio Gpio2(45, GPIO_Mode_In);
HWGpio Gpio3(42, GPIO_Mode_In);
HWGpio Gpio4(43, GPIO_Mode_In);
HWGpio LMotorDir(74, GPIO_Mode_Out); // CAN3_RX  left motor dir
HWGpio RMotorDir(75, GPIO_Mode_Out); // CAN3_TX  right motor dir

LS_I2C_DEV app(LS_IIC_PATH);

void LMotor_value(int duty)
{
    // LMotorPWM.SetDutyCycle(FLimit(duty, 0, motor_speed_max));
    LMotorPWM.SetDutyCycle(duty);

}

void RMotor_value(int duty)
{
    // RMotorPWM.SetDutyCycle(FLimit(duty, 0, motor_speed_max));
    RMotorPWM.SetDutyCycle(duty);
}

void LMotor_dir()
{
    LMotorDir.SetGpioValue(0);
}

void RMotor_dir()
{
    RMotorDir.SetGpioValue(1);
}

void Servo_value(int duty)
{
    servo.SetDutyCycle(FLimit(duty, servo_angle_min, servo_angle_max));
}


float L_encoder_value()
{
    return -EncodersL.Update();
}



float R_encoder_value()
{
    return EncodersR.Update();
}


int16_t Gyro_value(int t)
{
    if (t < 0 || t >= 6)
    {
        printf("Invalid index: %d\n", t);
        return -1;
    }
    int16_t data[6] = {0};
    app.I2C_Get_RawData(&data[0], &data[1], &data[2], &data[3], &data[4], &data[5]);//ax,ay,az,gx,gy,gz
    return data[t];
}

void Init_dev()
{
    LMotorPWM.Enable();
    RMotorPWM.Enable();
    servo.Enable();
    Servo_value(150000);
    LMotor_value(0);
    RMotor_value(0);
}

uint8_t key1()
{
    static bool lastKeyState = true; // 初始状态为未按下（高电平）
    bool currentKeyState = (Gpio1.GetGpioValue() == 0); // 低电平表示按下
    bool trigger = false;

    // 检测下降沿（从高到低的转换）
    if (currentKeyState && !lastKeyState) {
        trigger = true; // 检测到按键被按下
    }

    lastKeyState = currentKeyState; // 更新上一次状态
    return trigger ? 0 : 1; // 按下时返回0，其他情况返回1
}

uint8_t key44(){
    return Gpio1.GetGpioValue();
}


uint8_t key2()
{
    static bool lastKeyState = true; // 初始状态为未按下（高电平）
    bool currentKeyState = (Gpio2.GetGpioValue() == 0); // 低电平表示按下
    bool trigger = false;

    // 检测下降沿（从高到低的转换）
    if (currentKeyState && !lastKeyState) {
        trigger = true; // 检测到按键被按下
    }

    lastKeyState = currentKeyState; // 更新上一次状态
    return trigger ? 0 : 1; // 按下时返回0，其他情况返回1
}

uint8_t key45(){
    return Gpio2.GetGpioValue();
}

uint8_t key3()
{
    static bool lastKeyState = true; // 初始状态为未按下（高电平）
    bool currentKeyState = (Gpio3.GetGpioValue() == 0); // 低电平表示按下
    bool trigger = false;

    // 检测下降沿（从高到低的转换）
    if (currentKeyState && !lastKeyState) {
        trigger = true; // 检测到按键被按下
    }

    lastKeyState = currentKeyState; // 更新上一次状态
    return trigger ? 0 : 1; // 按下时返回0，其他情况返回1
}

uint8_t key42(){
    return Gpio3.GetGpioValue();
}

uint8_t key4()
{
    static bool lastKeyState = true; // 初始状态为未按下（高电平）
    bool currentKeyState = (Gpio4.GetGpioValue() == 0); // 低电平表示按下
    bool trigger = false;

    // 检测下降沿（从高到低的转换）
    if (currentKeyState && !lastKeyState) {
        trigger = true; // 检测到按键被按下
    }

    lastKeyState = currentKeyState; // 更新上一次状态
    return trigger ? 0 : 1; // 按下时返回0，其他情况返回1
}

uint8_t key43(){
    return Gpio4.GetGpioValue();
}